/*
 * Project name: PMA Regeneratief Remmen

 * Copyright:

 * Description:
	A simple version of an AC motor drive. Speed ramps from 5-40Hz and back down.
 * Test configuration:

 * NOTES:
     - . PIN Configuration								*ON MICROCBOARD
		RA0 ANALOG 	INPUT  ON/OFF 		SWITCH     		//SPEED POT
		RA1 ANALOG 	INPUT  LEFT/RIGHT 	SWITCH			//BREAK POT
		RA2 ANALOG 	INPUT  SPEED POTMETER
		RA3 ANALOG 	INPUT  BREAK POTMETER				//TIMER0 TESTPIN
		RA4 DIGITAL INPUT  C1OUT						//OUTPUT
		RA5 DIGITAL INPUT  C2OUT
		RA6 DIGITAL OUTPUT OSC1
		RA7 DIGITAL OUTPUT OSC2
		
		RB0 DIGITAL OUTPUT  LCD D4						//TIMER1 TESTPIN
		RB1 DIGITAL OUTPUT  LCD D6 						//LED1
		RB2 DIGITAL OUTPUT	LCD	D5						//LED2
		RB3 DIGITAL OUTPUT	LCD D4						//LED3
		RB4 DIGITAL OUTPUT	LCD E						//LED4
		RB5 DIGITAL OUTPUT  CCP3 PWM OUTPUT3			//PWM3
		RB6 DIGITAL OUTPUT 	LCD VO						//LED5
		RB7 DIGITAL OUTPUT  --          				//REMWEERSTAND
		
		RC0 DIGITAL OUTPUT 	--
		RC1 DIGITAL OUTPUT  CCP2 PWM OUTPUT2			//PWM2
		RC2 DIGITAL OUTPUT 	CCP1 PWM OUTPUT1			//PWM1
		RC3 DIGITAL OUTPUT 	--
		RC4 DIGITAL OUTPUT  USART SDI/SDA
		RC5 DIGITAL OUTPUT	USART SDO
		RC6 DIGITAL OUTPUT	USART TX
		RC7 DIGITAL OUTPUT	USART RX
		
		
*/
//Includes
#include "TMRxCOnfig.h"
#include "config.h"
#include "sine.h"

#define TRUE 1
#define FALSE 0


//Define LED Ouputs
#define LED1 PORTB.F1
#define LED2 PORTB.F2
#define LED3 PORTB.F3
#define LED4 PORTB.F4
#define LED5 PORTB.F6
#define TESTPIN PORTB.F0
#define TESTPIN2 PORTA.F3

#define REMWEERSTAND PORTB.F7

//Direction Values
#define NODIR     0
#define LEFTDIR   1
#define RIGHTDIR  2



//Speed Values
#define MINSPEED 5
#define DRIVESPEED  40


enum States  {
DRIVING,
BREAKING,
STANDSTILL,
ACC
}programState=ACC;


//------------Global Variables---------------------//
 unsigned short iSpeed=0,BreakON=FALSE, tmr1Overflow=FALSE,tmr0Overflow=FALSE, pwmOn=FALSE,timerValue =0;
 int n_position = 0;
 int n2_position=0,n3_position=0;
 unsigned short int PWMSignal[3];                    // Create a array with 3 elements for the pwm signals
 double tPeriod,tcp,t1Freq, t0Freq = 7812.50, acceleration=10.00;
 double  currentSpeed = MINSPEED, newSpeed = DRIVESPEED,unitstep, unitTime ;
 unsigned int h_value=0, TIMER1V=0, iDirection=0;
 unsigned short int getFreqInADC(double);
//Read AN Input Values
 unsigned short int l_value=0, newDirection=NODIR,currentDirection = NODIR;

 // Prototypes
 void sineWave(double);
 void portInit();
void checkControlPanel();
void updateDutyCycle(unsigned short int , unsigned short int);
double getFreqInHz(unsigned short , double ) ;
void calcUnitTime(double , double );

void initMain() {
  // INIT PORT CONFIGURATIONS
	portInit();

	sinetableInit();           // Initialize the sinetabel

	Pwm1_Init(5000);          //PWM INIt
	Pwm2_Init(5000);
	Pwm3_Init(5000);
  Timer1_Init();           //Timer Inits

  Timer0_OptionReg(0x87);  //0x84
  calcUnitTime(acceleration,t0Freq);
  Timer0_IE();
  TMR0 = timerValue;

  INTCON.GIE =1;           //Enable Interrupts
  INTCON.PEIE =1;
 }

void portInit()
{
/*
// NOTES:
//-------------------------
// TRISx BIT = 0 : BIT IS OUTPUT
//  ''  ''   = 1 : BIT IS INPUT
//-------------------------
*/
   //      TRISA = 0b00100111;
   TRISA = 0b11110111;
   PORTA = 0x00;
 

   //TRISB = 0b11000001;
   TRISB = 0b00000000;
   PORTB = 0x00;

   TRISC = 0b10111001;      // Outputs: RC1 and RC2
   PORTC = 0x00;            // Inputs: RC5
}

void main() {
   ADCON1 = 0b10001101;  // Configure analog inputs and Vref   //Changed
  initMain();

  while (1)
  {

	 switch (programState)
	{//preconditions: initial state = default (OFF state) ??
   
		case ACC: 
		/* ACCeleration from |fmin| to |freq| in |acc/dec time| seconds
		* PWM's	:ON
		* ACC	:ON
		* Timer0:ON
		* Timer1:ON
		*/
  		   //Output: Only Led3 On
			LED1 =0;
			LED2 =1;
			LED3 =1;
			LED4 =0;
			LED5 =0;
			
			REMWEERSTAND =0;

			if(pwmON==FALSE)
			{
				Pwm1_Start();
				Pwm2_Start();
				Pwm3_Start();
				Timer1_IE() ;
				Timer1_Start();
				pwmON=TRUE;
			}
			
			
			
			if(tmr0Overflow ==TRUE)
			{//unitStep must be already calculated
				
      if(currentSpeed<newSpeed)
            currentSpeed = currentSpeed+unitStep;
            
				sineWave(currentSpeed);
			  iSpeed=	getFreqInADC(currentSpeed);
				tmr0Overflow =FALSE;
				Timer0_IE();
			}
			
			//if speed is |freq| then programState = DRIVING
			if(currentSpeed >=newSpeed)
			{
				programState=DRIVING;
				Timer0_ID();
			}
			

			break;
		case DRIVING: 
		/* Driving at DRIVINGSPEED : This state is called after ACC
		* PWM's	:ON
		* ACC	:OFF
		* Timer0:OFF
		* Timer1:ON
		*/
			checkControlPanel();
			
			//Output: Led1, Led3 On
			LED1 =1;
			LED2 =0;
			LED3 =1;
			LED4 =0;
			LED5 =0;
			
			REMWEERSTAND =0;
           
			

			
			if(pwmON==FALSE)
			{
				Pwm1_Start();
				Pwm2_Start();
				Pwm3_Start();
				Timer1_IE() ;
				Timer1_Start();
				pwmON=TRUE;
			}
			
			//If break=ON then programState = BREAKING
			if(BreakON == TRUE)
			{
				programState=BREAKING;
				newSpeed= MINSPEED;
				Timer0_IE();
			}
			break;
		case BREAKING :  
			/* Breaking from |fmin| to |freq| in |acc/dec time| seconds
			* PWM's	:ON
			* ACC/DEC	:ON
			* Timer0:ON
			* Timer1:ON
			*/
			//Output: Led2, Led 3 On
			LED1 =0;
			LED2 =1;
			LED3 =1;
			LED4 =1;
			LED5 =1;
			
			REMWEERSTAND =1;
			
      if(pwmON==FALSE)
      {
        Pwm1_Start();
        Pwm2_Start();
        Pwm3_Start();
        Timer1_IE() ;
        Timer1_Start();
        pwmON=TRUE;
      }


			checkControlPanel();
			//If speed == |fmin| && Break ==OFF then programState ==ACC 
			
			
			if(tmr0Overflow ==TRUE)
			{//unitStep must be already calculated
				
        if(currentSpeed>newSpeed)
              currentSpeed = currentSpeed-unitStep;
            
				sineWave(currentSpeed);
        iSpeed=	getFreqInADC(currentSpeed);
        tmr0Overflow =FALSE;
				Timer0_IE();
			}
			
			//if speed is |freq| then programState = DRIVING
			/* if(currentSpeed <=newSpeed && BreakON==FALSE)
			{
				programState=STANDSTILL;
				
			} */
			
			if(currentSpeed <=newSpeed)
			{
				programState=STANDSTILL;
				REMWEERSTAND =0;
			}

			
			break;
			
			case STANDSTILL: 
			/* STANDSTILL at MINSPEED : This state is called after BREAKING
			* PWM's	:OFF
			* ACC	:OFF
			* Timer0:OFF
			* Timer1:ON
			*/
			checkControlPanel();
			
			//Output: Led1, Led3 On
			LED1 =1;
			LED2 =1;
			LED3 =1;
			LED4 =1;
			LED5 =1;
			
			REMWEERSTAND =0;
			
			if(pwmON==FALSE)
			{
				Pwm1_Start();
				Pwm2_Start();
				Pwm3_Start();
				Timer1_IE() ;
				Timer1_Start();
				pwmON=TRUE;
			}
            


			//If break=ON then programState = BREAKING
			if(BreakON == FALSE)
			{
				programState=ACC;
			
				currentSpeed = MINSPEED;
				newSpeed= DRIVESPEED;
				Timer0_IE();
			}
			break;
		default://OFF State

			//Outputs: ALl off
			LED1 =0;
			LED2 =0;
			LED3 =0;
			LED4 =0;
			LED5 =0;
			break;
	}

}
}
void calcUnitTime(double a, double tmr0Freq )
{
	double tmp=0;
    unitstep =0.25;
    unitTime = unitstep/a;
    tmp =    unitTime*tmr0Freq;
    timerValue = 0xFF - tmp;
}

void checkControlPanel()
{// Preconditions:
// - ADCON be setup accordingly
// - Switches be correctly configured
  
  //Read AN Input Values
    // iSpeed = Adc_Read(0)>>2;
     iDirection = Adc_Read(1);
	BreakON = 	(iDirection>255) ?TRUE :FALSE;
	//newSpeed = getFreqInHz(iSpeed,43.00);
  
}

double getFreqInHz(unsigned short adcSpeed, double MAXFREQ)
{
	double freq=0;
	double ratioFreq= (255.00)/MAXFREQ;

	if(adcSpeed ==255.00)
		freq = MAXFREQ;
	else
		freq= (double)adcSpeed/ratioFreq;

	return freq;

}

unsigned short int getFreqInADC(double Speed)
{
	unsigned short int adcfreq=0;
	double ratioFreq= (43.00)/255;

	if(Speed >=43.00)
		adcfreq = 255;
	else
		adcfreq= (double)Speed/ratioFreq;

	return adcfreq;

}

void sineWave(double frequency)
{  //preconditions : frequency <=43 hz
  unsigned short int tmr1Prescale=8;
 //  unsigned int h_value;

    //Calculate TMR1H & TMR1L
     tPeriod = (double)1/ frequency;
     tcp = tPeriod/ (TABLELEN);         //changed TABLELEN -> TABLELEN-1
     t1Freq = (double)FOSC/(4*tmr1Prescale);
   TIMER1V = (65535)-(tcp*t1Freq);
   l_value= (unsigned short int)TIMER1V & 0x00FF;
  h_value = (TIMER1V & 0xFF00)>>8;
}



void updateDutyCycle(unsigned short int dir, unsigned short freq)
{ //PWMSignal, n_position exists
  unsigned short int temp;
  
  // Frequency profile
  // if(freq<=3.00)
		// freq =0;
	// else if(freq <=10)
		// factor = getVoltInADC(MINFREQ*ratioVHz,MAXVOLT,bitsVolt)/255.00;
	// else if(freq == 255)
		// factor =1;
	// else
		// factor = getVoltInADC(freq*ratioVHz,MAXVOLT,bitsVolt)/255.00;
  
  
	switch(dir)
	{
			case LEFTDIR:
				//getDutyCycles(PWMSignal,n_position); //Get Dutycycle Values
        n_position++;
		 n_position = (n_position>=TABLELEN) ?(n_position-TABLELEN) :n_position ;		

				Pwm1_Change_Duty((PWMSignal[0]*freq)>>8);
				Pwm2_Change_Duty((PWMSignal[1]*freq)>>8);
				Pwm3_Change_Duty((PWMSignal[2]*freq)>>8);
			break;

			case RIGHTDIR:
			//	getDutyCycles(PWMSignal,n_position); //Get Dutycycle Values
				//getDutyCycles(PWMSignal,9);      // position 9 for testing purposes
				temp = PWMSignal[0]; //PWM1
				PWMSignal[0] = PWMSignal[1]; //PWM1 = PWM2
				PWMSignal[1] = temp;   //PWM2 = PWM1

				Pwm1_Change_Duty((PWMSignal[0]*freq)>>8);
				Pwm2_Change_Duty((PWMSignal[1]*freq)>>8);
				Pwm3_Change_Duty((PWMSignal[2]*freq)>>8);
			break;

			default:
				Pwm1_Change_Duty(0);
				Pwm2_Change_Duty(0);
				Pwm3_Change_Duty(0);
			break;
	}

}


 void interrupt()
 {
 unsigned int temp=0,temp2=(PR2+1) ;
   //Timer1 Interrupt
  if(PIR1.TMR1IF)
  {
  
  TESTPIN = ~(TESTPIN);
  // Timer1_Stop();
  TMR1H = h_value;
  TMR1L = l_value;
 // Timer1_Start();

	n2_position = n_position+OFFSET;
	n2_position = (n2_position >=TABLELEN)  ?(n2_position-TABLELEN)   :n2_position ;    //Range n2_position: 0 -> (TABLELEN-1)
	n3_position = n2_position+OFFSET;
	n3_position = (n3_position >= TABLELEN) ?(n3_position-TABLELEN)   :n3_position ;    //Range n3_position: 0 -> (TABLELEN-1)
	
  temp =sineValues[n_position]*temp2;
  temp = temp>>6;
  //temp = (temp*iSpeed)>>8;
  CCPR1L = temp>>2;              // get 8 MSB's
	CCP1CON.CCP1Y  = (temp&0x01) ?1 :0 ;   // get 2 LSB's
	CCP1CON.CCP1X  = (temp&0x02) ?1 :0;
	

  
  temp =sineValues[n2_position]*temp2;
  temp = temp>>6;
  //temp = (temp*iSpeed)>>8;
	CCPR2L = temp>>2;                      // get 8 MSB's
	CCP2CON.CCP2Y  = (temp&0x01) ?1 :0 ;   // get 2 LSB's
	CCP2CON.CCP2X  = (temp&0x02) ?1 :0;

  temp =sineValues[n3_position]*temp2;
  temp = temp>>6;
  //temp = (temp*iSpeed)>>8;
	CCPR3L = temp>>2;                      // get 8 MSB's
	CCP3CON.CCP3Y  = (temp&0x01) ?1 :0 ;   // get 2 LSB's
	CCP3CON.CCP3X  = (temp&0x02) ?1 :0;
  n_position++;
	n_position = (n_position>=TABLELEN) ?(n_position-TABLELEN) :n_position ;


  
   TESTPIN = ~(TESTPIN);
  
  PIR1.TMR1IF =0; //Clear Flag


  }else if(INTCON.TMR0IF)
  {
    TESTPIN2 = ~(TESTPIN2);


    TMR0 =timerValue;
    INTCON.TMR0IF =0;
    Timer0_ID();
    tmr0Overflow =TRUE;
  }


 }



